#include "Rotation.h"
#include "Angles.h"
#include "Quaternions.h"
#include "Math.h"
#include "Matrix4.h"

namespace Core
{
	inline rtRotation::rtRotation(void) 
	{
	}

	inline rtRotation::rtRotation(const rtVector3& rotationOrigin, const rtVector3& rotationVec, const float rotationAngle) 
	{
		m_Origin = rotationOrigin;
		m_Vec = rotationVec;
		m_Angle = rotationAngle;
		m_AxisValid = false;
	}

	inline void rtRotation::set(const rtVector3& rotationOrigin, const rtVector3& rotationVec, const float rotationAngle) 
	{
		m_Origin = rotationOrigin;
		m_Vec = rotationVec;
		m_Angle = rotationAngle;
		m_AxisValid = false;
	}

	inline void rtRotation::setOrigin(const rtVector3& rotationOrigin) 
	{
		m_Origin = rotationOrigin;
	}

	inline void rtRotation::setVec(const rtVector3& rotationVec) 
	{
		m_Vec = rotationVec;
		m_AxisValid = false;
	}

	inline void rtRotation::setVec(float x, float y, float z) 
	{
		m_Vec[0] = x;
		m_Vec[1] = y;
		m_Vec[2] = z;
		m_AxisValid = false;
	}

	inline void rtRotation::setAngle(const float rotationAngle) 
	{
		m_Angle = rotationAngle;
		m_AxisValid = false;
	}

	inline void rtRotation::scale(const float s) 
	{
		m_Angle *= s;
		m_AxisValid = false;
	}

	inline void rtRotation::reCalculateMatrix(void) 
	{
		m_AxisValid = false;
		toMat3();
	}

	inline const rtVector3& rtRotation::getOrigin(void) const 
	{
		return m_Origin;
	}

	inline const rtVector3& rtRotation::getVec(void) const  
	{
		return m_Vec;
	}

	inline float rtRotation::getAngle(void) const  
	{
		return m_Angle;
	}

	inline rtRotation rtRotation::operator - () const 
	{
		return rtRotation(m_Origin, m_Vec, -m_Angle);
	}

	inline rtRotation rtRotation::operator * (const float s) const 
	{
		return rtRotation(m_Origin, m_Vec, m_Angle * s);
	}

	inline rtRotation rtRotation::operator / (const float s) const 
	{
		assert(s != 0.0f);
		return rtRotation(m_Origin, m_Vec, m_Angle / s);
	}

	inline rtRotation &rtRotation::operator *= (const float s) 
	{
		m_Angle *= s;
		m_AxisValid = false;
		return *this;
	}

	inline rtRotation &rtRotation::operator /= (const float s) 
	{
		assert(s != 0.0f);
		m_Angle /= s;
		m_AxisValid = false;
		return *this;
	}

	inline rtVector3 rtRotation::operator * (const rtVector3& v) const 
	{
		if (!m_AxisValid) 
		{
			toMat3();
		}
		return ((v - m_Origin) * m_Axis + m_Origin);
	}

	inline rtRotation operator * (const float s, const rtRotation& r) 
	{
		return r * s;
	}

	inline rtVector3 operator * (const rtVector3& v, const rtRotation& r) 
	{
		return r * v;
	}

	inline rtVector3& operator *= (rtVector3& v, const rtRotation& r) 
	{
		v = r * v;
		return v;
	}

	inline void rtRotation::rotatePoint(rtVector3& point) const 
	{
		if (!m_AxisValid) 
		{
			toMat3();
		}
		point = ((point - m_Origin) * m_Axis + m_Origin);
	}
	
	rtAngles rtRotation::toAngles(void) const 
	{
		return toMat3().toAngles();
	}

	

	rtQuaternions rtRotation::toQuat(void) const 
	{
		float a, s, c;

		a = m_Angle * (rtMath::M_DEG2RAD * 0.5f);
		rtMath::SinCos(a, s, c);
		return rtQuaternions(m_Vec.x * s, m_Vec.y * s, m_Vec.z * s, c);
	}

	

	const rtMatrix3 &rtRotation::toMat3(void) const 
	{
		float wx, wy, wz;
		float xx, yy, yz;
		float xy, xz, zz;
		float x2, y2, z2;
		float a, c, s, x, y, z;

		if (m_AxisValid) 
		{
			return m_Axis;
		}

		a = m_Angle * (rtMath::M_DEG2RAD * 0.5f);
		rtMath::SinCos(a, s, c);

		x = m_Vec[0] * s;
		y = m_Vec[1] * s;
		z = m_Vec[2] * s;

		x2 = x + x;
		y2 = y + y;
		z2 = z + z;

		xx = x * x2;
		xy = x * y2;
		xz = x * z2;

		yy = y * y2;
		yz = y * z2;
		zz = z * z2;

		wx = c * x2;
		wy = c * y2;
		wz = c * z2;

		m_Axis[0][0] = 1.0f - (yy + zz);
		m_Axis[0][1] = xy - wz;
		m_Axis[0][2] = xz + wy;

		m_Axis[1][0] = xy + wz;
		m_Axis[1][1] = 1.0f - (xx + zz);
		m_Axis[1][2] = yz - wx;

		m_Axis[2][0] = xz - wy;
		m_Axis[2][1] = yz + wx;
		m_Axis[2][2] = 1.0f - (xx + yy);

		m_AxisValid = true;

		return m_Axis;
	}

	
	rtMatrix4 rtRotation::toMat4(void) const
	{
		return toMat3().toMat4();
	}

	
	rtVector3 rtRotation::toAngularVelocity(void) const
	{
		return m_Vec * DEG2RAD(m_Angle);
	}

	
	void rtRotation::normalize180(void) 
	{
		m_Angle -= floor(m_Angle / 360.0f) * 360.0f;
		if (m_Angle > 180.0f) 
		{
			m_Angle -= 360.0f;
		}
		else if (m_Angle < -180.0f) 
		{
			m_Angle += 360.0f;
		}
	}

	

	void rtRotation::normalize360(void) 
	{
		m_Angle -= floor(m_Angle / 360.0f) * 360.0f;
		if (m_Angle > 360.0f) 
		{
			m_Angle -= 360.0f;
		}
		else if (m_Angle < 0.0f) 
		{
			m_Angle += 360.0f;
		}
	}

}

